#include <path_planning_study/global_planning_node.h>

/*********************************************************************
 * Astar
 **********************************************************************/
// Astar 初始化
void GlobalPlanning::AstarInit(){
    __astar_planner.SetDebugMode(false);
}

// 设置地图数据
void GlobalPlanning::SetAstarMap(cv::Mat &image){
    __astar_planner.SetMap(image);
}

// Astar 路径规划
void GlobalPlanning::AstarPath(geometry_msgs::Pose start, geometry_msgs::Pose end){
    ros::Time start_time = ros::Time::now();

    // Astar
    vector<cv::Point> expand_poinds;    // 扩展点, 用于可视化
    cv::Point start_point = Pose2CvPoint(start);
    cv::Point goal_point = Pose2CvPoint(end);
    vector<cv::Point> astar_path = __astar_planner.GetPath(start_point, goal_point, expand_poinds);
    
    //计算耗时
    double time_cost = (ros::Time::now() - start_time).toSec();
    ROS_INFO("Astar planning path, time cost=%0.3f", time_cost);

    // 发布路径
    PublishPathOrigin(astar_path);
    // 发布扩展点
    PublishExpendPointsMap(expand_poinds);
}


/*********************************************************************
 * JPS
 **********************************************************************/
// Astar 初始化
void GlobalPlanning::JPSInit(){
    __jps_planner.SetDebugMode(false);
}

// 设置地图数据
void GlobalPlanning::SetJPSMap(cv::Mat &image){
    __jps_planner.SetMap(image);
}

// Astar 路径规划
void GlobalPlanning::JPSPath(geometry_msgs::Pose start, geometry_msgs::Pose end){
    ros::Time start_time = ros::Time::now();

    // Astar
    vector<cv::Point> expand_poinds;    // 扩展点, 用于可视化
    vector<cv::Point> jump_points;
    cv::Point start_point = Pose2CvPoint(start);
    cv::Point goal_point = Pose2CvPoint(end);
    vector<cv::Point> astar_path = __jps_planner.GetPath(start_point, goal_point, expand_poinds, jump_points);
    
    //计算耗时
    double time_cost = (ros::Time::now() - start_time).toSec();
    ROS_INFO("JPS planning path, time cost=%0.3f", time_cost);

    // 发布路径
    PublishPathOrigin(astar_path);
    // 发布扩展点
    PublishExpendPointsMap(expand_poinds);
    // 发布跳点数据
    PublishJumpPoints(jump_points);
}

// 发布跳点数据
void GlobalPlanning::PublishJumpPoints(vector<cv::Point>& jump_points){
    nav_msgs::GridCells grid_cells;
    grid_cells.header = __map_ptr->header;
    grid_cells.cell_height = 0.05;
    grid_cells.cell_width = 0.05;
    for(auto& point_item: jump_points){
        geometry_msgs::Pose pose = CvPoint2Pose(point_item);
        grid_cells.cells.push_back(pose.position);
    }
    __jps_jump_points_pub.publish(grid_cells);
}